/**
 * *********************************************************
 *
 * @file: path_planner_node.h
 * @brief: Contains the path planner ROS wrapper class
 * @author: Yang Haodong
 * @date: 2024-9-24
 * @version: 2.0
 *
 * Copyright (c) 2024, Yang Haodong.
 * All rights reserved.
 *
 * --------------------------------------------------------
 *
 * ********************************************************
 */
#ifndef RMP_PATH_PLANNER_PATH_PLANNER_NODE_H_
#define RMP_PATH_PLANNER_PATH_PLANNER_NODE_H_

#include <ros/ros.h>
#include <nav_core/base_global_planner.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/GetPlan.h>

#include "common/geometry/point.h"
#include "path_planner/path_planner.h"
#include "path_planner/utils/path_planner_factory.h"

namespace rmp {
namespace path_planner {
class PathPlannerNode : public nav_core::BaseGlobalPlanner {
public:
  /**
   * @brief Construct a new Path Planner object
   */
  PathPlannerNode();

  /**
   * @brief Construct a new Path Planner object
   * @param name        planner name
   * @param costmap_ros the cost map to use for assigning costs to trajectories
   */
  PathPlannerNode(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

  /**
   * @brief Destroy the Path Planner object
   */
  ~PathPlannerNode() = default;

  /**
   * @brief Planner initialization
   * @param name       planner name
   * @param costmapRos costmap ROS wrapper
   */
  void initialize(std::string name, costmap_2d::Costmap2DROS* costmapRos);

  /**
   * @brief Planner initialization
   * @param name planner name
   */
  void initialize(std::string name);

  /**
   * @brief Plan a path given start and goal in world map
   * @param start start in world map
   * @param goal  goal in world map
   * @param plan  plan
   * @return true if find a path successfully, else false
   */
  bool makePlan(const geometry_msgs::PoseStamped& start,
                const geometry_msgs::PoseStamped& goal,
                std::vector<geometry_msgs::PoseStamped>& plan);

  /**
   * @brief Plan a path given start and goal in world map
   * @param start     start in world map
   * @param goal      goal in world map
   * @param plan      plan
   * @param tolerance error tolerance
   * @return true if find a path successfully, else false
   */
  bool makePlan(const geometry_msgs::PoseStamped& start,
                const geometry_msgs::PoseStamped& goal, double tolerance,
                std::vector<geometry_msgs::PoseStamped>& plan);
  /**
   * @brief Regeister planning service
   * @param req  request from client
   * @param resp response from server
   */
  bool makePlanService(nav_msgs::GetPlan::Request& req,
                       nav_msgs::GetPlan::Response& resp);

protected:
  /**
   * @brief Calculate plan from planning path
   * @param path path generated by global planner
   * @param plan plan transfromed from path, i.e. [start, ..., goal]
   * @return bool true if successful, else false
   */
  bool _getPlanFromPath(const common::geometry::Points3d& path,
                        std::vector<geometry_msgs::PoseStamped>& plan);

protected:
  bool initialized_;                        // initialization flag
  costmap_2d::Costmap2DROS* costmap_ros_;   // costmap(ROS wrapper)
  std::string frame_id_;                    // costmap frame ID
  PLANNER_TYPE planner_type_;               // planner type
  std::shared_ptr<PathPlanner> g_planner_;  // global path planner
  ros::Publisher plan_pub_;                 // path planning publisher
  ros::Publisher expand_pub_;               // nodes explorer publisher
  ros::Publisher points_pub_;               // key-points publisher
  ros::Publisher lines_pub_;                // polygons publisher
  ros::Publisher tree_pub_;                 // random search tree publisher
  ros::Publisher particles_pub_;            // evolutionary particles publisher
  ros::ServiceServer make_plan_srv_;        // planning service
};
}  // namespace path_planner
}  // namespace rmp
#endif